{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import sys  ###resetmcl1\n",
    "sys.path.append('../scripts/')\n",
    "from mcl import *"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "class ResetMcl(Mcl): \n",
    "    def __init__(self, envmap, init_pose, num, motion_noise_stds={\"nn\":0.19, \"no\":0.001, \"on\":0.13, \"oo\":0.2}, \\\n",
    "                 distance_dev_rate=0.14, direction_dev=0.05):\n",
    "        super().__init__(envmap, init_pose, num, motion_noise_stds, distance_dev_rate, direction_dev)\n",
    "        self.alphas = {}  #α値の記録（ランドマークの観測数ごと）\n",
    "        \n",
    "    def observation_update(self, observation):\n",
    "        for p in self.particles:\n",
    "            p.observation_update(observation, self.map, self.distance_dev_rate, self.direction_dev) \n",
    "        \n",
    "        #alpha値の記録\n",
    "        alpha = sum([p.weight for p in self.particles])\n",
    "        obsnum = len(observation)\n",
    "        if not obsnum in self.alphas: self.alphas[obsnum] = []\n",
    "        self.alphas[obsnum].append(alpha)\n",
    "            \n",
    "        self.set_ml()\n",
    "        self.resampling() #ここで重みの合計は1になる"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "def trial():\n",
    "    time_interval = 0.1\n",
    "    world = World(300, time_interval, debug=True) \n",
    "\n",
    "    ### 地図を生成して3つランドマークを追加 ###\n",
    "    m = Map()                                  \n",
    "    m.append_landmark(Landmark(-4,2))\n",
    "    m.append_landmark(Landmark(2,-3))\n",
    "    m.append_landmark(Landmark(3,3))\n",
    "    world.append(m)          \n",
    "\n",
    "    ### ロボットを作る ###\n",
    "    initial_pose = np.array([0, 0, 0]).T\n",
    "    pf = ResetMcl(m, initial_pose, 100)\n",
    "    circling = EstimationAgent(time_interval, 0.2, 10.0/180*math.pi, pf)\n",
    "    r = Robot(initial_pose, sensor=Camera(m), agent=circling, color=\"red\")\n",
    "    world.append(r)\n",
    "\n",
    "    world.draw()\n",
    "    \n",
    "    return pf\n",
    "\n",
    "pf = trial()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "landmarks: 2 particles: 100 min: 0.22390594484416315 max: 17.764815065661896\n",
      "landmarks: 1 particles: 100 min: 0.04456137572738368 max: 13.004374720939975\n",
      "landmarks: 0 particles: 100 min: 1.0000000000000007 max: 1.0000000000000007\n"
     ]
    }
   ],
   "source": [
    "for num in pf.alphas: ###mclalpharesult\n",
    "    print(\"landmarks:\", num, \"particles:\", len(pf.particles), \"min:\", min(pf.alphas[num]), \"max:\", max(pf.alphas[num]))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "def trial():\n",
    "    time_interval = 0.1\n",
    "    world = World(40, time_interval, debug=True) \n",
    "\n",
    "    ### 地図を生成して3つランドマークを追加 ###\n",
    "    m = Map()                                  \n",
    "    m.append_landmark(Landmark(-4,2))\n",
    "    m.append_landmark(Landmark(2,-3))\n",
    "    m.append_landmark(Landmark(3,3))\n",
    "    world.append(m)          \n",
    "\n",
    "    ### ロボットを作る ###\n",
    "    pf = ResetMcl(m, np.array([-4, -4, 0]).T, 100)\n",
    "    circling = EstimationAgent(time_interval, 0.2, 10.0/180*math.pi, pf)\n",
    "    r = Robot(np.array([0,0,0]).T, sensor=Camera(m), agent=circling, color=\"red\")\n",
    "    world.append(r)\n",
    "\n",
    "    world.draw()\n",
    "    \n",
    "    return pf\n",
    "    \n",
    "pf = trial()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "landmarks: 2 particles: 100 min: 0.0 max: 1.3466655425634616e-93\n",
      "landmarks: 1 particles: 100 min: 2.3373005364474248e-269 max: 5.793366598372977\n",
      "landmarks: 0 particles: 100 min: 1.0000000000000007 max: 1.0000000000000007\n"
     ]
    }
   ],
   "source": [
    "for num in pf.alphas: ###mclalpharesult2\n",
    "    print(\"landmarks:\", num, \"particles:\", len(pf.particles), \"min:\", min(pf.alphas[num]), \"max:\", max(pf.alphas[num]))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "{2: [8.946435289371282e-144,\n",
       "  1.3466655425634616e-93,\n",
       "  1.1969463785125035e-97,\n",
       "  6.420143544461477e-298,\n",
       "  0.0,\n",
       "  0.0,\n",
       "  0.0,\n",
       "  0.0,\n",
       "  0.0,\n",
       "  0.0,\n",
       "  0.0,\n",
       "  0.0,\n",
       "  0.0,\n",
       "  0.0,\n",
       "  0.0,\n",
       "  0.0,\n",
       "  0.0,\n",
       "  0.0,\n",
       "  0.0,\n",
       "  0.0,\n",
       "  0.0,\n",
       "  0.0,\n",
       "  0.0,\n",
       "  9.386105190351529e-301,\n",
       "  1.534075061425e-312,\n",
       "  3.1979511714764463e-296,\n",
       "  2.6847285169515614e-276],\n",
       " 1: [1.0463613821796277e-112,\n",
       "  2.0063060607720784e-07,\n",
       "  1.0488631743586148e-06,\n",
       "  0.003245487078127109,\n",
       "  4.4495288350830396e-05,\n",
       "  0.00047654905457778743,\n",
       "  4.892812482410206e-05,\n",
       "  7.441507868232858e-05,\n",
       "  0.0020697774326909895,\n",
       "  0.006022909810809369,\n",
       "  0.00146855371628662,\n",
       "  0.00027443595901814545,\n",
       "  0.0006345450339847864,\n",
       "  0.0003666688417901743,\n",
       "  0.0008940685335963873,\n",
       "  0.00022966457451834005,\n",
       "  0.00028330699885764963,\n",
       "  0.0024594107205467134,\n",
       "  0.0006087242133377529,\n",
       "  0.00025195848767216626,\n",
       "  0.00020979327531830993,\n",
       "  0.00037815502867614856,\n",
       "  0.0041507766803532175,\n",
       "  0.0009885495474145389,\n",
       "  0.00014552732012136546,\n",
       "  0.0002433419917043072,\n",
       "  0.00025813093177812327,\n",
       "  3.936560581654454e-05,\n",
       "  0.00043164224922015963,\n",
       "  0.00013910528279514265,\n",
       "  0.0017394324137165306,\n",
       "  0.0001501928086908009,\n",
       "  0.0033096439818610227,\n",
       "  0.0005445792165672732,\n",
       "  0.00030203076238570774,\n",
       "  9.742551907077837e-06,\n",
       "  0.00033079460841583397,\n",
       "  0.00040603865483295105,\n",
       "  0.0001892005936735634,\n",
       "  0.0001351377396965511,\n",
       "  0.0002971934378712066,\n",
       "  0.00012737579905803455,\n",
       "  0.0002468346227031982,\n",
       "  0.00017075824753451127,\n",
       "  0.0005686583846153287,\n",
       "  0.00011199394275323007,\n",
       "  5.0106989076897415e-05,\n",
       "  0.0001802859203550205,\n",
       "  6.097534904340346e-05,\n",
       "  2.7903079222281398e-05,\n",
       "  6.795169566431164e-05,\n",
       "  0.00010209228910024511,\n",
       "  0.0006440952767512841,\n",
       "  0.0011181996517922825,\n",
       "  5.112572162061103e-05,\n",
       "  0.00104259150486411,\n",
       "  1.9065673108426488e-05,\n",
       "  0.0006457611584856863,\n",
       "  0.00024786652543149824,\n",
       "  0.00012562386349193662,\n",
       "  0.0003099022929483214,\n",
       "  0.00024685278530327926,\n",
       "  0.0003108657667669234,\n",
       "  2.0305436158941042e-05,\n",
       "  0.00015996687480624607,\n",
       "  0.00030039252075460124,\n",
       "  0.0005333141833984134,\n",
       "  0.00021069600117698363,\n",
       "  0.0001950018513260868,\n",
       "  2.6546234219313587e-05,\n",
       "  0.00012743813025242168,\n",
       "  0.00015047912903973603,\n",
       "  0.0004840813513113862,\n",
       "  0.00013741072941045172,\n",
       "  0.000360435066038653,\n",
       "  7.778192991277835e-05,\n",
       "  0.00045968897988375463,\n",
       "  9.080108632510444e-05,\n",
       "  0.00026120586672612444,\n",
       "  0.00020772944741481874,\n",
       "  0.00012163531794913391,\n",
       "  0.00015819091789201023,\n",
       "  0.0004530692819732076,\n",
       "  0.00011085488951792257,\n",
       "  2.916709108172601e-05,\n",
       "  0.00018744882518789336,\n",
       "  3.0596834716685674e-05,\n",
       "  0.0001504814922585817,\n",
       "  0.00011353483923309953,\n",
       "  2.8271107223678793e-05,\n",
       "  0.0002341690153272045,\n",
       "  8.44585412589024e-05,\n",
       "  9.685709552295624e-05,\n",
       "  0.0004840141092480768,\n",
       "  9.399059473162576e-05,\n",
       "  1.4213776791523233e-39,\n",
       "  1.9037460918264e-27,\n",
       "  2.1358056862610273e-27,\n",
       "  2.6871135637232507e-19,\n",
       "  5.1056604977099385e-15,\n",
       "  1.907178994030418e-11,\n",
       "  2.1651965058288175e-06,\n",
       "  0.0021029136223046836,\n",
       "  2.27772685994552e-08,\n",
       "  0.016356591690527913,\n",
       "  0.032743018966175916,\n",
       "  0.000996596964019767,\n",
       "  0.017545966242351503,\n",
       "  0.08071552635922886,\n",
       "  0.934650117282382,\n",
       "  0.023226113366273997,\n",
       "  0.005363099764081757,\n",
       "  0.11489735089549956,\n",
       "  0.03581223286050306,\n",
       "  0.2983046198582039,\n",
       "  0.010733496609705752,\n",
       "  0.22360430153217875,\n",
       "  0.012875763739489602,\n",
       "  0.0022373835974133743,\n",
       "  0.3548704886060351,\n",
       "  0.06319341940651475,\n",
       "  0.08256952314136067,\n",
       "  0.06706597792009417,\n",
       "  1.4907968249596621,\n",
       "  0.010518647796983045,\n",
       "  1.7447992655586497,\n",
       "  0.16708156995817489,\n",
       "  0.14835521966036833,\n",
       "  0.5544874322714973,\n",
       "  2.0971311383666893,\n",
       "  3.2237818360673187,\n",
       "  0.7370819986735463,\n",
       "  0.019823698100139668,\n",
       "  0.02740202114639856,\n",
       "  0.05858703742155946,\n",
       "  0.052136353991518766,\n",
       "  0.6598127104852115,\n",
       "  0.3193108268157017,\n",
       "  1.4469410675079466,\n",
       "  3.1981347947257324,\n",
       "  1.8186356157395123,\n",
       "  2.178285004573944,\n",
       "  0.27494552109049913,\n",
       "  2.296873841847146,\n",
       "  2.612057283768086,\n",
       "  0.2756871889859137,\n",
       "  0.49633819078459457,\n",
       "  0.04822877864389984,\n",
       "  0.017620943141053686,\n",
       "  3.69098140706188,\n",
       "  4.1203594636368965,\n",
       "  0.14310924346663448,\n",
       "  0.8565279747027765,\n",
       "  0.6484548935846962,\n",
       "  2.3370253878130876,\n",
       "  3.398530487229662,\n",
       "  2.9619370120192547,\n",
       "  1.6496548213462845,\n",
       "  0.9373589762172304,\n",
       "  1.7892631057744097,\n",
       "  2.1612798410299514,\n",
       "  2.92492129261218,\n",
       "  2.1798923672322994,\n",
       "  3.9911849708481544,\n",
       "  2.3860202074682935,\n",
       "  0.23925524331005252,\n",
       "  1.9034407137478988,\n",
       "  0.4931048628698424,\n",
       "  2.3127045401612847,\n",
       "  2.7942299856230797,\n",
       "  0.02412552547446756,\n",
       "  3.2685588042478826,\n",
       "  1.4099787256112113,\n",
       "  3.530124458764958,\n",
       "  1.7859648984398717,\n",
       "  0.5037755450985912,\n",
       "  3.3719915464715373,\n",
       "  3.483509777040638,\n",
       "  0.5991242689322336,\n",
       "  3.225996950118226,\n",
       "  1.0856125514492454,\n",
       "  0.31240788330505215,\n",
       "  3.0146942046885488,\n",
       "  2.8872672387829996,\n",
       "  0.613554513973183,\n",
       "  3.5128620603884526,\n",
       "  2.182431722385706,\n",
       "  3.0143572132306016,\n",
       "  1.1587040729961464,\n",
       "  4.717179389887847,\n",
       "  1.6077977639559464,\n",
       "  4.5799368647836545,\n",
       "  4.007739107340924,\n",
       "  4.814659939604114,\n",
       "  3.3383230784183167,\n",
       "  3.851339313522921,\n",
       "  1.7333521830436678,\n",
       "  4.814979368791606,\n",
       "  2.9668229648634252,\n",
       "  4.4688780249533755,\n",
       "  2.21262985236642,\n",
       "  1.2407948229029182,\n",
       "  4.9143994615989985,\n",
       "  5.043300865292263,\n",
       "  3.9097633372819858,\n",
       "  3.8135061190271426,\n",
       "  3.2401222470243947,\n",
       "  2.469943807301704,\n",
       "  4.940796525303633,\n",
       "  3.839967017855631,\n",
       "  2.3827661461713183,\n",
       "  3.4727669658913536,\n",
       "  4.335242444085287,\n",
       "  4.515022471052569,\n",
       "  4.798204526271886,\n",
       "  3.3006068535977646,\n",
       "  4.870433533281555,\n",
       "  0.26411363934174337,\n",
       "  4.972623486291787,\n",
       "  3.5371798660616847,\n",
       "  3.6874089014026064,\n",
       "  2.565837143899195,\n",
       "  5.793366598372977,\n",
       "  1.2723784138949346,\n",
       "  3.289205415538446,\n",
       "  4.632822475858365,\n",
       "  5.392382493604482,\n",
       "  5.3326230446516805,\n",
       "  3.475108929316051,\n",
       "  2.4538100810797574,\n",
       "  5.317343440167682,\n",
       "  2.5853709342104344e-169,\n",
       "  4.3593930176253936e-243,\n",
       "  1.1308805500847943e-233,\n",
       "  3.745701390707732e-234,\n",
       "  2.9248526843037985e-232,\n",
       "  1.2001606974557184e-218,\n",
       "  3.8537954916836354e-231,\n",
       "  2.0238210389483752e-214,\n",
       "  1.7934976899204432e-218,\n",
       "  7.401464501001107e-181,\n",
       "  1.0106878088758034e-165,\n",
       "  1.0232190445895082e-189,\n",
       "  1.0870496409780927e-173,\n",
       "  1.1687595293542383e-179,\n",
       "  8.571730575134826e-164,\n",
       "  6.407150292782931e-162,\n",
       "  4.205174391819733e-176,\n",
       "  5.901460381867068e-169,\n",
       "  6.1019629629355725e-173,\n",
       "  8.162037280461128e-164,\n",
       "  1.0443313586596402e-159,\n",
       "  3.42859599627964e-172,\n",
       "  1.2891907652345074e-159,\n",
       "  2.234236256668467e-142,\n",
       "  4.550611680057544e-141,\n",
       "  2.7913965971552794e-145,\n",
       "  4.8692253443022534e-163,\n",
       "  4.037552930072373e-124,\n",
       "  5.306107250531098e-140,\n",
       "  5.101488676464474e-163,\n",
       "  8.357544709596638e-187,\n",
       "  3.7616685210720944e-163,\n",
       "  1.65779877213351e-163,\n",
       "  1.1607508554223324e-174,\n",
       "  6.962323876276025e-169,\n",
       "  1.1427604871761495e-170,\n",
       "  1.5288667818780356e-217,\n",
       "  2.4024355013805027e-198,\n",
       "  7.006645535660731e-202,\n",
       "  2.50557733723624e-184,\n",
       "  3.1888642650843035e-216,\n",
       "  2.8694076489891582e-198,\n",
       "  1.0704935081689302e-214,\n",
       "  9.27264954611174e-205,\n",
       "  1.431331276878087e-219,\n",
       "  3.391193640064911e-210,\n",
       "  4.1732135277750437e-196,\n",
       "  3.1244595006951846e-204,\n",
       "  7.89968217682578e-194,\n",
       "  1.9665066707551128e-194,\n",
       "  4.3475021162324195e-209,\n",
       "  3.224201794579072e-233,\n",
       "  1.174931505001261e-222,\n",
       "  9.424178689344996e-224,\n",
       "  1.5263902957853022e-222,\n",
       "  1.0769873290209358e-195,\n",
       "  2.3251826472225742e-208,\n",
       "  2.1463999922792122e-210,\n",
       "  2.556434428679043e-212,\n",
       "  6.835604829341727e-224,\n",
       "  4.956344797419177e-185,\n",
       "  2.5030247523024562e-185,\n",
       "  1.9553506049357276e-183,\n",
       "  8.727016566341319e-184,\n",
       "  6.567226752427432e-178,\n",
       "  4.5339404426929174e-161,\n",
       "  1.3384700434641814e-182,\n",
       "  1.3060984451945535e-153,\n",
       "  7.409743940960372e-155,\n",
       "  1.6558274356473167e-157,\n",
       "  8.471503536823087e-163,\n",
       "  8.532647835871458e-176,\n",
       "  5.170070529012677e-206,\n",
       "  5.725032435230367e-183,\n",
       "  8.563560677536047e-154,\n",
       "  2.147862166190632e-188,\n",
       "  3.6180868051214804e-196,\n",
       "  2.119426291652656e-208,\n",
       "  2.5558974941694375e-237,\n",
       "  6.919367186533567e-244,\n",
       "  1.1576182512721436e-246,\n",
       "  2.051961044436468e-234,\n",
       "  7.883016876725317e-247,\n",
       "  1.5763493642245911e-242,\n",
       "  6.506690023370441e-265,\n",
       "  2.3373005364474248e-269,\n",
       "  4.2998941182029067e-72],\n",
       " 0: [1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007,\n",
       "  1.0000000000000007]}"
      ]
     },
     "execution_count": 7,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "pf.alphas"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.0"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
